#include "robot_arm.h"

const float pi = 3.1415926f;
RobotAngle_Argument_t RobotAngle_Argument = {0};

// 初始化机械臂
void robotArm_init(void)
{

#if AUTOMATIC_RESET == 1
	
	stepMotor_power(1, 0);
	stepMotor_power(2, 0);
	stepMotor_power(3, 0);
	
	stepMotor_power(1, 1);

	stepMotor_power(3, 1);
	stepMotor_setSpeed(3, 1, ROBOT_ARM_INIT_SPEED);
	while (key_scan(2))
        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
	stepMotor_setSpeed(3, 1, 0);

	stepMotor_power(2, 1);
	stepMotor_setSpeed(2, 0, ROBOT_ARM_INIT_SPEED);
	while (key_scan(1))
        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
	stepMotor_setSpeed(2, 0, 0);
	
	stepMotor_setSpeed(1, 1, ROBOT_ARM_INIT_SPEED);
	while (key_scan(0))
	    STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
	stepMotor_setSpeed(1, 1, 0);

#endif	

	while (key_scan(0) || key_scan(1) || key_scan(2))
	{
		stepMotor_power(1, 0);
		stepMotor_power(2, 0);
		stepMotor_power(3, 0);
        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
	}

	ROBOT_ARM_DELAY(500);

	stepMotor_power(1, 1);
	stepMotor_power(2, 1);
	stepMotor_power(3, 1);

	stepMotor_clearZero(1);
	stepMotor_clearZero(2);
	stepMotor_clearZero(3);
	
	{
		stepMotor_setSpeed(3, 0, ROBOT_ARM_INIT_SPEED);
		while ((int)stepMotor_getAngle(3) <= ROBOT_ARM_INIT_AXLE_3)
	        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		stepMotor_setSpeed(3, 0, 0);
	}
	
	{
		stepMotor_setSpeed(2, 1, ROBOT_ARM_INIT_SPEED);
		while ((int)stepMotor_getAngle(2) >= ROBOT_ARM_INIT_AXLE_2)
	        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		stepMotor_setSpeed(2, 0, 0);
	}

	{
		stepMotor_setSpeed(1, 0, ROBOT_ARM_INIT_SPEED);  // 在药房的项目中两个机械臂的初始状态不一样
		while ((int)stepMotor_getAngle(1) <= ROBOT_ARM_INIT_AXLE_1)
	        STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		stepMotor_setSpeed(1, 0, 0);
	}
	
	stepMotor_clearZero(1);
	stepMotor_clearZero(2);
	stepMotor_clearZero(3);

	RobotAngle_Argument.arm_length_1 = 180;
	RobotAngle_Argument.arm_length_2 = 200;
	RobotAngle_Argument.arm_length_3 = 200;
	RobotAngle_Argument.axle_angle[0] = 0;
	RobotAngle_Argument.axle_angle[1] = 0;
	RobotAngle_Argument.axle_angle[2] = 0;
	
	ROBOT_ARM_DELAY(1000);
	
}
//
//RobotArmRun_t robotArm_assign(float x,float y,float z,uint16_t rtim,uint16_t num,uint16_t pwm,uint16_t ctim){
//
//	RobotArmRun_t robotArmRun;
//	robotArmRun.ctim = ctim;
//	robotArmRun.num = num;
//	robotArmRun.pwm = pwm;
//	robotArmRun.rtim = rtim;
//	robotArmRun.x = x;
//	robotArmRun.y = y;
//	robotArmRun.z = z;
//	return robotArmRun;
//
//}

RobotArmRun_t robotArm_assign(float x,float y,float z,uint16_t rtim,uint16_t num){

    RobotArmRun_t robotArmRun;
    robotArmRun.num = num;
    robotArmRun.rtim = rtim;
    robotArmRun.x = x;
    robotArmRun.y = y;
    robotArmRun.z = z;
    return robotArmRun;

}

int8_t robotArm_actionGroup(RobotArmRun_t * robotArmRun,uint16_t num){
	
	for(int i=0;i<num;i++){
		
		robotArm_kinematicsRun(robotArmRun[i]);
		
		while(0 != robotArm_getRunOK(ROBOT_BIAS)){// 调整偏差值
		    ROBOT_ARM_DELAY(100);
		}
//		key_KA( robotArmRun[i].num);  //机械臂控制吸泵
	}
	
	return 0;
	
}

void robotArm_control_time(uint16_t time)
{

	stepMotor_circle_time(1, ROBOT_ARM_BASE_GEAR_RATIO * RobotAngle_Argument.axle_angle[0], time);
	stepMotor_circle_time(3, ROBOT_ARM_RARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[2], time);
	//	while((int)stepMotor_getAngle(3) == ROBOT_ARM_RARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[2]);
	RobotAngle_Argument.axle_angle[1] -= 90.0f;
	RobotAngle_Argument.axle_angle[1] += RobotAngle_Argument.axle_angle[2];
	stepMotor_circle_time(2, ROBOT_ARM_LARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[1], time);
	
}

void robotArm_control(uint16_t speed)
{

	stepMotor_circle(1, ROBOT_ARM_BASE_GEAR_RATIO * RobotAngle_Argument.axle_angle[0], speed);
	stepMotor_circle(3, ROBOT_ARM_RARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[2], speed);
	//	while((int)stepMotor_getAngle(3) == ROBOT_ARM_RARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[2]);
	RobotAngle_Argument.axle_angle[1] -= 90.0f;
	RobotAngle_Argument.axle_angle[1] += RobotAngle_Argument.axle_angle[2];
	stepMotor_circle(2, ROBOT_ARM_LARM_GEAR_RATIO * RobotAngle_Argument.axle_angle[1], speed);
}

void robotArm_getAngle(float *angle0, float *angle1, float *angle2)
{
	*angle0 = stepMotor_getAngle(1) / 4.5f;
	*angle1 = stepMotor_getAngle(2) / -6.0f;
	*angle2 = stepMotor_getAngle(3) / 6.0f;
}

int robotArm_kinematicsRun(RobotArmRun_t robotArmRun){
	return robotArm_kinematics_move(robotArmRun.x,robotArmRun.y,robotArmRun.z,robotArmRun.rtim);
}

int robotArm_kinematicsControl(RobotArmMove_t robotArmMove,RobotArmClamp_t robotArmClamp){

	return robotArm_kinematics_move(robotArmMove.x,robotArmMove.y,robotArmMove.z,robotArmMove.time);
}

int robotArm_kinematics_move(float x, float y, float z, uint16_t time)
{
	int returnVal = 0;
	returnVal = robotArm_kinematics(x, y, z, &RobotAngle_Argument);
//	if (0 == returnVal)
//	{
		robotArm_control_time(time);
//	}
//	DBG_PRINT("Errro:%d\r\n", returnVal);
	return returnVal;
}

int robotArm_kinematics(float x, float y, float z, RobotAngle_Argument_t* robot_argument)
{
	float L0 = robot_argument->arm_length_1;
	float L1 = robot_argument->arm_length_2;
	float L2 = robot_argument->arm_length_3;

	float temp_A0 = 0;
	float temp_A1 = 0;
	float temp_A2 = 0;

	float temp_B1 = 0;
	float temp_B2 = 0;
	float temp_B4 = 0;
	float temp_B5 = 0;

	float temp_S1 = 0;
	float temp_S2 = 0;
	float temp_S3 = 0;

	RobotAngle_Error_t error = Not_Error;
	
	temp_A0 = robotArm_atan(x / y);
	
	if(temp_A0 > 90.0f || temp_A0 < -90.0f)	
		error = A0_Error;

	temp_S2 = sqrt( robotArm_square(x) + robotArm_square(y) );
	temp_S3 = sqrt( robotArm_square(z) + robotArm_square(temp_S2) );
	temp_B4 = robotArm_acos( z / temp_S3 );
	temp_S1 = sqrt( robotArm_square(L0) + robotArm_square(temp_S3) - (2 * L0 * temp_S3 * robotArm_cos(temp_B4)) );
	temp_B1 = robotArm_acos( (robotArm_square(L1) + robotArm_square(L2) - robotArm_square(temp_S1)) / (2 * L1 * L2));

	if(temp_B1 > 150.0f || temp_B1 < 45.0f)	
		error = A2_Error;

	temp_A2 = 180.0f - temp_B1;

	temp_B2 = robotArm_acos((robotArm_square(L1) + robotArm_square(temp_S1) - robotArm_square(L2)) / (2 * L1 * temp_S1));
	temp_B5 = robotArm_acos((robotArm_square(temp_S1) + robotArm_square(L0) - robotArm_square(temp_S3)) / (2 * temp_S1 * L0));

	temp_A1 = 180.0f - temp_B2 - temp_B5;

	if(temp_A1 > 80.0f || temp_A1 < -45.0f)	
		error = A1_Error;

	robot_argument->axle_angle[0] = temp_A0;
	robot_argument->axle_angle[2] = temp_A1;
	robot_argument->axle_angle[1] = temp_A2;

	return (int)error;
}

int robotArm_floatEquals(float a, float b, float epsilon) {
    return fabs(a - b) < epsilon;
}

int8_t robotArm_getRunOK(float bias){
	float angle_1 = 0,angle_2 = 0,angle_3 = 0;
	robotArm_getAngle(&angle_1,&angle_2,&angle_3);
//
//	LOG_W("Angle_1:%.2f  Angle_2:%.2f  Angle_3:%.2f  \r\n",RobotAngle_Argument.axle_angle[0],RobotAngle_Argument.axle_angle[1],RobotAngle_Argument.axle_angle[2]);
//	LOG_W("angle_1:%.2f  angle_2:%.2f  angle_3:%.2f  \r\n",angle_1,angle_2,angle_3);
//
	if(	robotArm_floatEquals(RobotAngle_Argument.axle_angle[0],angle_1,bias) &&
		robotArm_floatEquals(RobotAngle_Argument.axle_angle[1],angle_2,bias) &&
		robotArm_floatEquals(RobotAngle_Argument.axle_angle[2],angle_3,bias))
	{
		return 0;
	}
	else{

	    RobotAngle_Argument.axle_angle[0] = RobotAngle_Argument.axle_angle[0] >= angle_1 ? RobotAngle_Argument.axle_angle[0] - ROBOT_BIAS_ADJUST : RobotAngle_Argument.axle_angle[0] + ROBOT_BIAS_ADJUST;
        RobotAngle_Argument.axle_angle[1] = RobotAngle_Argument.axle_angle[1] >= angle_2 ? RobotAngle_Argument.axle_angle[1] - ROBOT_BIAS_ADJUST : RobotAngle_Argument.axle_angle[1] + ROBOT_BIAS_ADJUST;
        RobotAngle_Argument.axle_angle[2] = RobotAngle_Argument.axle_angle[2] >= angle_3 ? RobotAngle_Argument.axle_angle[2] - ROBOT_BIAS_ADJUST : RobotAngle_Argument.axle_angle[2] + ROBOT_BIAS_ADJUST;
        
		return -1;

	}
}

float robotArm_PID(float error) {
    // PID 参数
    float Kp = 0.25; // 比例系数
    float Ki = 0; // 积分系数
    float Kd = 0; // 微分系数

    // 误差累积
    static float integral = 0;
    // 上次误差
    static float previous_error = 0;

    // 计算比例项
    float proportional = Kp * error;

    // 计算积分项
    integral += error;
    float integral_term = Ki * integral;

    // 计算微分项
    float derivative = Kd * (error - previous_error);

    // 更新上次误差
    previous_error = error;

    // 计算 PID 调整值
    float output = proportional + integral_term + derivative;

    return output;
}

float robotArm_cos(float angle) {
	return 	cos(angle * pi / 180.0f);
}

float robotArm_sin(float angle) {
	return 	sin(angle * pi / 180.0f);
}

float robotArm_square(float val) {
	return 	val * val;
}

float robotArm_acos(float radian) {
	if (radian < -1.0f || radian > 1.0f) {
		return -1.0f;
	}
	return acos(radian) * 180.0f / pi;
}

float robotArm_atan(float radian) {
	return atan(radian) * 180.0f / pi;
}

void robotArm_AngleGo(float angle,float length,RobotArmRun_t * robotArmRun){
	float temp_S1 = 0;
	float temp_S2 = 0;
	
	temp_S1 = length * robotArm_sin(angle);
	temp_S2 = length * robotArm_cos(angle);
	
	robotArmRun->x += temp_S1;
	robotArmRun->y += temp_S2;
}












